#include <Servo.h> 
#include <SoftwareSerial.h>

//int ledPin = 13;      // LED connected to digital pin 9
SoftwareSerial mySerial(10, 11); // RX, TX
int val = 0;         // variable to store the read value
int direcPin = 8;
int farolPin = 12;
int backwardPin = 7;
int motorPin = 6;
int turboPin = 9;
int backlightPin = 13;
boolean lights = false;
Servo myservo;
boolean reading;
int speed = 0;
int servopos;
int rel;
int cont = 0;
int prev1 = 0;
int prev2 = 0;
boolean op = false;
int zero = 90; //85

void setup()
{
  //mySerial.begin(9600);
  mySerial.begin(9600);
  myservo.attach(8);
  pinMode(direcPin, OUTPUT);   // sets the pin as output
  pinMode(farolPin, OUTPUT); 
  pinMode(backwardPin, OUTPUT); 
  pinMode(motorPin, OUTPUT); 
  pinMode(turboPin, OUTPUT); 
  pinMode(13, OUTPUT); 
  stopMotor();
  turnCenter();
  for(int i = 0; i < 3; i++){
    frontLightOn();
    delay(500);
    frontLightOff();
    delay(500);
  } 
}

//ENGINE CONTROL

void goForward(){
     analogWrite(backwardPin, 0);
     analogWrite(motorPin, speed); 
}

void turboOn(){
    analogWrite(turboPin, 255);
}

void turboOff(){
      analogWrite(turboPin, 0);
}

void goBackward(){
    analogWrite(backwardPin, 255);
    analogWrite(motorPin, speed); 
}

void stopMotor(){
    analogWrite(motorPin, 0);
    analogWrite(turboPin, 0);
    analogWrite(backwardPin, 0);
   // speed = 100;
}

//STEERING CONTROL

void turn(int i){
  
  if( i < prev1 ){  
    if(prev1 < prev2){
       myservo.write(i);
    }
    prev2 = prev1;
    prev1 = i;
  }else{
    if(prev1 > prev2){
      myservo.write(i);
    }
    prev2 = prev1;
    prev1 = i;
  }
  
}

void turnLeft(){
  turn(zero+30);
}

void turnRight(){
  turn(zero-30);
}

void turnCenter(){
   turn(zero);
}

//LIGHTS CONTROL

void frontLightOn(){
    digitalWrite(farolPin, 170);
}

void frontLightOff(){
    digitalWrite(farolPin, 0);
}

void frontLightHigh(){
    digitalWrite(farolPin, 255);
}

void backLightOn(){
    digitalWrite(backlightPin, HIGH);
}

void backLightOff(){
    digitalWrite(backlightPin, LOW);
}

//Arduino LOOP

void loop(){
   
 if(mySerial.available()){
  int c = mySerial.read();
  
  if(c == '!')
    op = true;
  else
  if(op){//AQplicado a todo switch
  switch (c) {
  case '8':
    goForward(); 
    break;
 case '&':
    turboOff();
    break;
 
  case '0':
    stopMotor();
    break;
  case '/':
    turboOn();
    break;
      case '2':
      goBackward();
    break;
  case '-':
    frontLightOff();
    backLightOff();
    break;
  case '+':
    frontLightOn();
    backLightOn();
    break;
  case '4':
      turnLeft();
    break;
  case '5':
    turnCenter();
    break;
  case '6':
    turnRight();
    break;
  case '7':
    frontLightOn();
    break;
  case '9':
    frontLightOff();
    break;
  case '1':
    backLightOn();
    break;
  case '3':
    backLightOff();
    break;
  case '.':
    delay(100);
    break;
  case '*':
    delay(250);
    break;
  case '@':
    delay(500);
    break;
  default: 
    if(c >=65){
      
      if(c < 75 ){
        //Dobrar dir de 65 a 74
        rel = c - 65;
        int ang = zero + rel * 4;
        turn(ang);
        }else if(c == 75){
        turnCenter();
        // Exatamente 75
        }else if(c <= 85){
        //Dobrar esq de 76 a 85
        rel = c - 76;
        int ang = zero - rel * 4;
        turn(ang);
        } else if(c < 94){
        //acelerar
          speed = (c - 86) * 31;
          goForward();  
        }
        else if (c <102 ){
         //Espaço reservado as novas instrucoes 
          speed = (c - 94) * 31;
          goBackward(); 
        }
    }
    }
    op = false;
  }
    
}
  
  
  


}
